
	#include "QXCCommunications.h"

/******************************************************************************
*   						INTERNAL VARIABLES
******************************************************************************/
	#if DATA_STRUCTURE == TERMINAL
	static char DataTerminalBuffer[NUMBER_LINES*CARACTERS_LENGTH];
	#endif
	#if DATA_STRUCTURE == AHRS_GUI_TI || DATA_STRUCTURE == AHRS_GUI_QXC
	static char DataGUIBuffer[AHRS_DATA_LENGTH];
	#endif
	#if DATA_STRUCTURE == QXC_WIFI
	static char DataGUIBuffer[MESSAGE_LENGTH];
	#endif
	
	volatile char DataControlBuffer[30];
	volatile char *DataControlPtr;
	char DataControlEngage;
	char NewDataControl;
	
	volatile char MessageType=0;
	volatile unsigned short CaracterCounter=0;
	DisplaySM DisplayStatus;
	DisplaySM GUIStatus;
	DisplaySM QXSTStatus;
	DisplaySM QXINStatus;
	DisplaySM QXDBStatus;
	DisplaySM QXTIStatus;
	volatile short HeightCM_t,ProcessorTemperature_t;
	_iq BatteryStatus_t;

/******************************************************************************
*   					LOCAL FUNCTIONS DEFINITIONS
******************************************************************************/	
	void TerminalDisplayStateFill (void);
	void TerminalDisplayStateDisplay(void);
	void TerminalDisplayStateFinsh (void);
	
	void EulerAnlgesSection (void);
	void SetEulerAnglesSection (void);
	void HeightSection (void);
	void OnBoardTemperatureSection (void);
	void BatterySection (void);
	void MotorsThrustSection (void);
	void ControlTypeSection (void);
	void EnviromentSensorSection (void);
	void PIDValuesSection (void);
	void PIDErrorValuesSection (void);
	
	// Introduce character Message Limiters at the begining and the end, <MESSAGE> //
	void VfnMessageLimiters (void);
	
	// DataGUIBuffer errase  function	//
	void VfnGUIBufferErase (void);
	
	// Function that split the Messag and, gives number of data splited and position
	void VfnParsedData(char *BufferPtr,unsigned char ParseCounter, char Spliter, unsigned char* DataCounter, char *DataPosition);

	// atoi Function	//
	short VfnMyAtoi(char* BufferPtr, unsigned char Counter);
	
	// Main state Machine to Send a Data Message throug UART port //
	void (* const TerminalDisplay[])(void);
	// State Machine to Fill up a Buffer wiht a specific Message information //
	// QXST Message Type
	void (* const FillQXSTTypeBuffer[]) (void);
	// QXIN Message Type
	void (* const FillQXINTypeBuffer[]) (void);
	// QXDB Message Type
	void (* const FillQXDBTypeBuffer[]) (void);
	// QXTI Message Type
	void (* const FillQXTITypeBuffer[]) (void);

/******************************************************************************
*   						LOCAL FUNCTIONS
******************************************************************************/		
 /*****************************************************************************
 * Function:	VfnDisplayDataIntoTerminal
 * Description:	Function/"EVENT" that run the State Machine to write "the System Status"/GUI sequence into a PC Terminal
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	void VfnDisplayDataIntoTerminal (void)
	{
		TerminalDisplay[DisplayStatus.ActualState]();
//		if(DisplayStatus.ActualState == STATE_FINISH)
//		{
//			//DisplayStatus.ActualState = STATE_FILL;
//		}	
	}
	
	void VfnDisplayDataInitialization (void)
	{
		DisplayStatus.ActualState=0;
		GUIStatus.ActualState=0;	
		QXSTStatus.ActualState=0;
		QXINStatus.ActualState=0;
		QXDBStatus.ActualState=0;
		QXTIStatus.ActualState=0;
	}

 /*****************************************************************************
 * Function:	VfnMessageManager 
 * Description:	Function that handle all messages coming from the server. Clissifies and take actions.
 * Parameters:	Are pointer to variable locations where to store the new commands.
 * 				*NOTE: new commands are avalibale only when function return a number 2.
 * Caveats:		
 * Return:		(1) when Server is requesting data, (0) when Server Stop requesting and, (2) when Server send New Commands
 *****************************************************************************/		
	char VfnMessageManager (unsigned char Port, signed char* SetRoll, signed char* SetPitch, signed char* SetYaw, signed short* SetHeight, char* Hold)
	{
		DataControlPtr = &DataControlBuffer[0];
		
		// Detect if first part of header is CS "Control Server"
		if(*DataControlPtr == 'C' && *((DataControlPtr)+1) == 'S')
		{
			// Detect is second part of header is IR "Information Request"
			if(*((DataControlPtr)+2) == 'I' && *((DataControlPtr)+3) == 'R')
			{
				DataControlPtr = &DataControlBuffer[5];
				// Detect first Message type == QXST
				// *Note - Only check for the first, third and fourth byte of the second part of the message
				if(*(DataControlPtr) == 'Q' && *((DataControlPtr)+2) == 'S' && *((DataControlPtr)+3) == 'T' )
				{
					MessageType = QXST;
					VfnGUIBufferErase();
					return 1;
				}
				// Detect first Message type == QXIN -> 
				if(*(DataControlPtr) == 'Q' && *(DataControlPtr+2) == 'I' && *(DataControlPtr+3) == 'N' )
				{
					MessageType = QXIN;
					VfnGUIBufferErase();
					return 1;
				}
				// Detect first Message type == QXDB -> Debug information
				if(*(DataControlPtr) == 'Q' && *(DataControlPtr+2) == 'D' && *(DataControlPtr+3) == 'B' )
				{
					MessageType = QXDB;
					VfnGUIBufferErase();
					return 1;
				}
				// Detect first Message type == QXTI -> Total information
				if(*(DataControlPtr) == 'Q' && *(DataControlPtr+2) == 'T' && *(DataControlPtr+3) == 'I' )
				{
					MessageType = QXTI;
					VfnGUIBufferErase();
					return 1;
				}
				
				// Detect first Message type == QXMI -> Main information
				if(*(DataControlPtr) == 'Q' && *(DataControlPtr+2) == 'M' && *(DataControlPtr+3) == 'I' )
				{
					MessageType = QXMI;
					VfnGUIBufferErase();
					return 1;
				}
				// Detect first Message type == QXSP -> Stop sending messages
				if(*(DataControlPtr) == 'Q' && *(DataControlPtr+2) == 'S' && *(DataControlPtr+3) == 'P' )
				{
					MessageType = QXSP;
					return 0;
				}
			}
			
			// Detect is second part of header is CI "Commands Inputa"
			if(*(DataControlPtr+2) == 'C' && *(DataControlPtr+3) == 'I')
			{
				volatile char CounterD;
				volatile char PositionD[10];
				VfnParsedData(&DataControlBuffer[0],30,',',&CounterD,&PositionD[0]);
				
				*SetRoll = (signed char)VfnMyAtoi(&DataControlBuffer[ PositionD[1]],((PositionD[2]-PositionD[1])-1) );
				
				*SetPitch = (signed char)VfnMyAtoi(&DataControlBuffer[ PositionD[2]],((PositionD[3]-PositionD[2])-1) );
				
				*SetYaw = (signed char)VfnMyAtoi(&DataControlBuffer[ PositionD[3]],((PositionD[4]-PositionD[3])-1) );
				
				*SetHeight = (signed short)VfnMyAtoi(&DataControlBuffer[ PositionD[4]],((PositionD[5]-PositionD[4])-1) );
				
				*Hold = (char)VfnMyAtoi(&DataControlBuffer[ PositionD[5]],((PositionD[6]-PositionD[5])-1) );
				
				return 2;
			}
			
		}
		return 0;	
	}

/*****************************************************************************
 * Function:	VfnMyAtoi
 * Description:	Convert a string number into a int/short number
 * Parameters:	String "Pointer" and, number of caracter to convert.
 * Caveats:		
 * Return:		number "Short type variable" / 0 if nothing to convert
 *****************************************************************************/
	short VfnMyAtoi(char* BufferPtr, unsigned char Counter)
	{
		unsigned char Count=0;
		volatile short number=0;
		volatile short Sign=1;
		while(Count != Counter)
		{
			if(*BufferPtr != '-')
			{
				if( *BufferPtr<'0' || *BufferPtr>'9')
				{
					return number;	
				}
				else
				{
					number = number * 10 + *BufferPtr - '0';
				}
				BufferPtr++;
				Count++;
			}
			else
			{
				BufferPtr++;
				Count++;
				Sign = -1;	
			}
		}
		return (number * Sign);
	}
	
/*****************************************************************************
 * Function:	TerminalDisplayStateFill
 * Description:	STATE 0 - of the State machine that sends data through the UART0 Port into a PC terminal
 * 				STATE FILL - Load all data into DataTerminalBuffer.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayStateFill (void)
	{
		_iq valor;	
	#if DATA_STRUCTURE == TERMINAL
		unsigned char thrust;	
		sprintf((char*)&DataTerminalBuffer[ROLL_LINE*CARACTERS_LENGTH] , "ROLL        = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_LINE*CARACTERS_LENGTH], "PITCH       = ");
		sprintf((char*)&DataTerminalBuffer[YAW_LINE*CARACTERS_LENGTH]  , "YAW         = ");
		sprintf((char*)&DataTerminalBuffer[TEMP_LINE*CARACTERS_LENGTH] , "TEMPERATURE = ");
		sprintf((char*)&DataTerminalBuffer[PTEMP_LINE*CARACTERS_LENGTH] , "ARM   TEMP. = ");
		sprintf((char*)&DataTerminalBuffer[BATTERY_LINE*CARACTERS_LENGTH] , "BATTERY VOLT= ");
		sprintf((char*)&DataTerminalBuffer[HEIGHT_LINE*CARACTERS_LENGTH] , "HEIGHT cm.  = ");
		
		thrust =  VfnGetQuadThrust();
		sprintf((char*)&DataTerminalBuffer[THRUST_LINE*CARACTERS_LENGTH],"QXC Thrust  = %d %%",thrust);
		
//		if(ControlCommandType == ANALOG_COMMANDS)
//		{
//			sprintf((char*)&DataTerminalBuffer[COMMAND_LINE*CARACTERS_LENGTH] , "COMMANDS TYP= Analog");
//		}
//		if(ControlCommandType == DIGITAL_COMMANDS)
//		{
//			sprintf((char*)&DataTerminalBuffer[COMMAND_LINE*CARACTERS_LENGTH] , "COMMANDS TYP= Digital");
//		}
		sprintf((char*)&DataTerminalBuffer[SET_ROLL_LINE*CARACTERS_LENGTH] , "SET ROLL    = ");
		sprintf((char*)&DataTerminalBuffer[SET_PITCH_LINE*CARACTERS_LENGTH] ,"SET PITCH   = ");
		sprintf((char*)&DataTerminalBuffer[SET_YAW_LINE*CARACTERS_LENGTH] ,  "SET YAW     = ");
		

		sprintf((char*)&DataTerminalBuffer[ROLL_ERROR_LINE*CARACTERS_LENGTH] , "ROLL ERROR  = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_ERROR_LINE*CARACTERS_LENGTH] ,"PITCH ERROR = ");
		sprintf((char*)&DataTerminalBuffer[YAW_ERROR_LINE*CARACTERS_LENGTH] ,  "YAW ERROR   = ");
		sprintf((char*)&DataTerminalBuffer[ROLL_PID_LINE*CARACTERS_LENGTH] , "ROLL PID    = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_PID_LINE*CARACTERS_LENGTH] ,"PITCH PID   = ");
		sprintf((char*)&DataTerminalBuffer[YAW_PID_LINE*CARACTERS_LENGTH] ,  "YAW PID     = ");
		
		DataTerminalBuffer[((PITCH_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((TEMP_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((PTEMP_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((BATTERY_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((HEIGHT_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((THRUST_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((THRUST_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		DataTerminalBuffer[((COMMAND_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_PITCH_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_ROLL_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		DataTerminalBuffer[((PITCH_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((PITCH_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_PID_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(ROLL_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(PITCH_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(YAW_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		VfnDS16XXTemperatureToString(&DataTerminalBuffer[(TEMP_LINE*CARACTERS_LENGTH)+DATA_START],DS1624Temp);
		sprintf((char*)&DataTerminalBuffer[(PTEMP_LINE*CARACTERS_LENGTH)+DATA_START] ,  "%d",ProcessorTemperature_t);
		_IQtoa(&DataTerminalBuffer[(BATTERY_LINE*CARACTERS_LENGTH)+DATA_START],"%4.3f",BatteryStatus_t);
		sprintf((char*)&DataTerminalBuffer[(HEIGHT_LINE*CARACTERS_LENGTH)+DATA_START] ,  "%d",HeightCM_t);
		
		valor = _IQmpy(SetRollAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_ROLL_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(SetPitchAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_PITCH_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(SetYawAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_YAW_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		
		_IQtoa(&DataTerminalBuffer[(PITCH_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorPitch);
		_IQtoa(&DataTerminalBuffer[(ROLL_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorRoll);
		_IQtoa(&DataTerminalBuffer[(YAW_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorYaw);
		
		_IQtoa(&DataTerminalBuffer[(PITCH_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidP);
		_IQtoa(&DataTerminalBuffer[(ROLL_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidR);
		_IQtoa(&DataTerminalBuffer[(YAW_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidY);
		DisplayStatus.ActualState = STATE_DISPLAY;
	#endif
	
	#if DATA_STRUCTURE == AHRS_GUI_TI
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ROLL_ANGLE],"%5.4f",valor);
		DataGUIBuffer[PITCH_ANGLE-1] = ',';
	
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[PITCH_ANGLE],"%5.4f",valor);
		DataGUIBuffer[YAW_ANGLE-1] = ',';	
		
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[YAW_ANGLE],"%5.4f",valor);
	
		DataGUIBuffer[AHRS_DATA_LENGTH-1] = '!';
		DisplayStatus.ActualState = STATE_DISPLAY;
	#endif	
	
	#if DATA_STRUCTURE == AHRS_GUI_QXC
		unsigned char thrust;
		volatile unsigned short Motors[4];
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ROLL_ANGLE],"%5.4f",valor);
		DataGUIBuffer[PITCH_ANGLE-1] = ',';
	
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[PITCH_ANGLE],"%5.4f",valor);
		DataGUIBuffer[YAW_ANGLE-1] = ',';	
		
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[YAW_ANGLE],"%5.4f",valor);
		DataGUIBuffer[SET_ROLL-1] = ',';	
		
		valor = _IQmpy(SetRollAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[SET_ROLL],"%3.5f",valor);
		DataGUIBuffer[SET_PITCH-1] = ',';
		
		valor = _IQmpy(SetPitchAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[SET_PITCH],"%3.5f",valor);
		DataGUIBuffer[SET_YAW-1] = ',';
		
		valor = _IQmpy(SetYawAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[SET_YAW],"%3.5f",valor);
		DataGUIBuffer[SET_THRUST-1] = ',';
		
		thrust =  VfnGetQuadThrust();
		sprintf((char*)&DataGUIBuffer[SET_THRUST],"%d",thrust);
		DataGUIBuffer[BOARD_TEMPERATURE-1] = ',';
		VfnDS16XXTemperatureToString(&DataGUIBuffer[BOARD_TEMPERATURE],DS1624Temp);
		DataGUIBuffer[PID_ROLL-1] = ',';
		
		_IQtoa(&DataGUIBuffer[PID_ROLL],"%3.4f",PidR);
		DataGUIBuffer[PID_PITCH-1] = ',';
		_IQtoa(&DataGUIBuffer[PID_PITCH],"%3.4f",PidP);
		DataGUIBuffer[PID_YAW-1] = ',';
		_IQtoa(&DataGUIBuffer[PID_YAW],"%3.4f",PidY);
		DataGUIBuffer[PID_HEIGHT-1] = ',';
		valor=_IQ(0);
		_IQtoa(&DataGUIBuffer[PID_HEIGHT],"%3.5f",valor);
		DataGUIBuffer[MOTOR_X1-1] = ',';
		
		VfnGetMotorsThrust(&Motors[0]);
		sprintf((char*)&DataGUIBuffer[MOTOR_X1],"%d",Motors[0]);
		DataGUIBuffer[MOTOR_X2-1] = ',';
		sprintf((char*)&DataGUIBuffer[MOTOR_X2],"%d",Motors[1]);
		DataGUIBuffer[MOTOR_Y1-1] = ',';
		sprintf((char*)&DataGUIBuffer[MOTOR_Y1],"%d",Motors[2]);
		DataGUIBuffer[MOTOR_Y2-1] = ',';
		sprintf((char*)&DataGUIBuffer[MOTOR_Y2],"%d",Motors[3]);
		
		DataGUIBuffer[AHRS_DATA_LENGTH-1] = '!';
		DisplayStatus.ActualState = STATE_DISPLAY;
	#endif
		
	#if DATA_STRUCTURE == QXC_WIFI
		if( MessageType == QXST )
		{
			FillQXSTTypeBuffer[QXSTStatus.ActualState]();
			if(QXSTStatus.ActualState == SECTION7)
			{
				DataGUIBuffer[ ((CONTROL_TYPE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters();
				VfnDisplayDataInitialization();
				DisplayStatus.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageType == QXIN )
		{
			FillQXINTypeBuffer[QXINStatus.ActualState]();
			if(QXINStatus.ActualState == SECTION2)
			{
				DataGUIBuffer[ ((TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters();
				VfnDisplayDataInitialization();
				DisplayStatus.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageType == QXDB )
		{
			FillQXDBTypeBuffer[QXDBStatus.ActualState]();
			if(QXDBStatus.ActualState == SECTION3)
			{
				DataGUIBuffer[((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters();
				VfnDisplayDataInitialization();
				DisplayStatus.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageType == QXTI )
		{
			FillQXTITypeBuffer[QXTIStatus.ActualState]();
			if(QXTIStatus.ActualState == SECTION_MAX)
			{
				DataGUIBuffer[((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters();
				VfnDisplayDataInitialization();
				DisplayStatus.ActualState = STATE_DISPLAY;
			}
		}
	#endif
	}

/*****************************************************************************
 * Function:	TerminalDisplayStateDisplay
 * Description:	STATE 1 - of the State machine that sends data through the UART0 Port into a PC terminal
 * 				STATE DISPLAY - Sends all data contain in the DataTerminalBuffer[] through the UART0 port.
 * 				MAX_DISPLAY determine the number of caracters send by the UART0 port.
 * 				once caracter counter has reach MAX_DISPLAY. avance to next state "Finish"
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayStateDisplay (void)
	{
		char FIFOFlag;
	#if DATA_STRUCTURE == TERMINAL
		//if Actual data in the buffer is equal to 0, then skip data.
		if(DataTerminalBuffer[CaracterCounter]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataTerminalBuffer[CaracterCounter]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				CaracterCounter++;	
				if(CaracterCounter == MAX_DISPLAY)
				{
					// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
					CaracterCounter=0;
					DisplayStatus.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounter++;
		}
	#endif
	#if DATA_STRUCTURE == AHRS_GUI_TI || DATA_STRUCTURE == AHRS_GUI_QXC
	//if Actual data in the buffer is equal to 0, then skip data.
		if(DataGUIBuffer[CaracterCounter]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataGUIBuffer[CaracterCounter]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				CaracterCounter++;	
				if(CaracterCounter == (AHRS_DATA_LENGTH))
				{
					// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
					CaracterCounter=0;
					DisplayStatus.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounter++;
		}
	#endif
	
	#if DATA_STRUCTURE == QXC_WIFI
	//if Actual data in the buffer is equal to 0, then skip data.
		if(DataGUIBuffer[CaracterCounter]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataGUIBuffer[CaracterCounter]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
				CaracterCounter++;	
				if(CaracterCounter == (MESSAGE_LENGTH))
				{
					CaracterCounter=0;
					DisplayStatus.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounter++;
		}
	#endif
	}
	
/*****************************************************************************
 * Function:	TerminalDisplayStateFinish
 * Description:	STATE 2 - of the State machine that sends data through the UART0 Port into a PC terminal
 * 				STATE FINISH - Reset CaracterCounter.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayStateFinsh (void)
	{
		CaracterCounter=0;
	}
	
/*****************************************************************************
 * Function:	void (* const TerminalDisplay[])(void)
 * Description:	Functions Pointer - that conform the TerminalDisplay State Machine.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void (* const TerminalDisplay[])(void) =
	{
		TerminalDisplayStateFill,
		TerminalDisplayStateDisplay,
		TerminalDisplayStateFinsh
	};

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
#if DATA_STRUCTURE == QXC_WIFI
	void EulerAnlgesSection (void)
	{
		_iq valor;
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ ((ROLL_ANGLE * DATA_LENGTH)+1)+5 ],"%4.2f",valor);
		DataGUIBuffer[ ((ROLL_ANGLE*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
	
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ (PITCH_ANGLE * DATA_LENGTH)+5 ],"%4.2f",valor);
		DataGUIBuffer[ ((PITCH_ANGLE*DATA_LENGTH)+DATA_LENGTH)+4] = ',';	
		
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ (YAW_ANGLE * DATA_LENGTH)+5 ],"%4.2f",valor);
		DataGUIBuffer[ ((YAW_ANGLE*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		//Next State
		QXSTStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void SetEulerAnglesSection (void)
	{
		_iq valor;
		valor = _IQmpy(SetRollAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ (SET_ROLL * DATA_LENGTH)+5 ],"%4.2f",valor);
		DataGUIBuffer[ ((SET_ROLL*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		valor = _IQmpy(SetPitchAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ (SET_PITCH * DATA_LENGTH)+5 ],"%4.2f",valor);
		DataGUIBuffer[ ((SET_PITCH*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		valor = _IQmpy(SetYawAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ (SET_YAW * DATA_LENGTH)+5 ],"%4.2f",valor);
		DataGUIBuffer[ ((SET_YAW*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXSTStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void HeightSection (void)
	{
		//_IQtoa(&DataGUIBuffer[ (HEIGHT * DATA_LENGTH)+5 ],"%4.2f",HeightCM_t);
		sprintf((char*)&DataGUIBuffer[(HEIGHT * DATA_LENGTH)+5 ],"%d",HeightCM_t);
		DataGUIBuffer[ ((HEIGHT*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		//_IQtoa(&DataGUIBuffer[ (SET_HEIGHT * DATA_LENGTH)+5 ],"%4.2f",_IQ(0.0));
		sprintf((char*)&DataGUIBuffer[(SET_HEIGHT * DATA_LENGTH)+5 ],"%d",0);
		DataGUIBuffer[ ((SET_HEIGHT*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXSTStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void OnBoardTemperatureSection (void)
	{
		VfnDS16XXTemperatureToString(&DataGUIBuffer[(BOARD_TEMPERATURE * DATA_LENGTH)+5],DS1624Temp);	
		DataGUIBuffer[ ((BOARD_TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXSTStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void BatterySection (void)
	{
		_IQtoa(&DataGUIBuffer[ (BATTERY * DATA_LENGTH)+5 ],"%4.2f",BatteryStatus_t);
		//sprintf((char*)&DataGUIBuffer[(BATTERY * DATA_LENGTH)+5 ],"%d",0);
		DataGUIBuffer[ ((BATTERY*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXSTStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
		QXINStatus.ActualState +=1;
		QXDBStatus.ActualState +=1;
	}
	
	void MotorsThrustSection (void)
	{
		unsigned char thrust;
		volatile unsigned short Motors[4];
		thrust =  VfnGetQuadThrust();
		sprintf((char*)&DataGUIBuffer[(THRUST * DATA_LENGTH)+5 ],"%d",thrust);
		DataGUIBuffer[ ((THRUST*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		sprintf((char*)&DataGUIBuffer[(SET_THRUST * DATA_LENGTH)+5 ],"%d",0);
		DataGUIBuffer[ ((SET_THRUST*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		VfnGetMotorsThrust(&Motors[0]);
		sprintf((char*)&DataGUIBuffer[(MOTOR_X1 * DATA_LENGTH)+5 ],"%d",Motors[0]);
		DataGUIBuffer[ ((MOTOR_X1*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		sprintf((char*)&DataGUIBuffer[(MOTOR_X2 * DATA_LENGTH)+5 ],"%d",Motors[1]);
		DataGUIBuffer[ ((MOTOR_X2*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		sprintf((char*)&DataGUIBuffer[(MOTOR_Y1 * DATA_LENGTH)+5 ],"%d",Motors[2]);
		DataGUIBuffer[ ((MOTOR_Y1*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		sprintf((char*)&DataGUIBuffer[(MOTOR_Y2 * DATA_LENGTH)+5 ],"%d",Motors[3]);
		DataGUIBuffer[ ((MOTOR_Y2*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXSTStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void ControlTypeSection (void)
	{
		//  Falta el Control Type
		DataGUIBuffer[(CONTROL_TYPE * DATA_LENGTH)+5] = '1';
		DataGUIBuffer[ ((CONTROL_TYPE*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXSTStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void EnviromentSensorSection (void)
	{
		sprintf((char*)&DataGUIBuffer[(CO_GAS * DATA_LENGTH)+5 ],"%d",1);
		DataGUIBuffer[ ((CO_GAS*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		sprintf((char*)&DataGUIBuffer[(O3_GAS * DATA_LENGTH)+5 ],"%d",2);
		DataGUIBuffer[ ((O3_GAS*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		sprintf((char*)&DataGUIBuffer[(HUMIDITY * DATA_LENGTH)+5 ],"%d",3);
		DataGUIBuffer[ ((HUMIDITY*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		sprintf((char*)&DataGUIBuffer[(TEMPERATURE * DATA_LENGTH)+5 ],"%d",4);
		DataGUIBuffer[ ((TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXINStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void PIDValuesSection (void)
	{
		_IQtoa(&DataGUIBuffer[(PID_ROLL * DATA_LENGTH)+5 ],"%4.2f",PidR);
		DataGUIBuffer[((PID_ROLL*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		_IQtoa(&DataGUIBuffer[(PID_PITCH * DATA_LENGTH)+5 ],"%4.2f",PidP);
		DataGUIBuffer[((PID_PITCH*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		_IQtoa(&DataGUIBuffer[(PID_YAW * DATA_LENGTH)+5 ],"%4.2f",PidY);
		DataGUIBuffer[((PID_YAW*DATA_LENGTH)+DATA_LENGTH)+4] = ',';	
		//Actializar variable del PID_HEIGHT
		_IQtoa(&DataGUIBuffer[(PID_HEIGHT * DATA_LENGTH)+5 ],"%4.2f",_IQ(0.0));
		DataGUIBuffer[((PID_HEIGHT*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXDBStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;
	}
	
	void PIDErrorValuesSection (void)
	{
		_IQtoa(&DataGUIBuffer[(ROLL_ERROR * DATA_LENGTH)+5 ],"%2.5f",ErrorRoll);
		DataGUIBuffer[((ROLL_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		_IQtoa(&DataGUIBuffer[(PITCH_ERROR * DATA_LENGTH)+5 ],"%2.5f",ErrorPitch);
		DataGUIBuffer[((PITCH_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		_IQtoa(&DataGUIBuffer[(YAW_ERROR * DATA_LENGTH)+5 ],"%2.5f",ErrorYaw);
		DataGUIBuffer[((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = ',';
		
		QXDBStatus.ActualState +=1;
		QXTIStatus.ActualState +=1;	
	}

	// QXST Message Type
	void (* const FillQXSTTypeBuffer[]) (void)=
	{
		EulerAnlgesSection,
		SetEulerAnglesSection,
		HeightSection,
		OnBoardTemperatureSection,
		BatterySection,
		MotorsThrustSection,
		ControlTypeSection
	};
	// QXIN Message Type
	void (* const FillQXINTypeBuffer[]) (void) =
	{
		EnviromentSensorSection,
		BatterySection
	};
	// QXDB Message Type
	void (* const FillQXDBTypeBuffer[]) (void)=
	{
		PIDValuesSection,
		PIDErrorValuesSection,
		BatterySection
	};
	// QXTI Message Type
	void (* const FillQXTITypeBuffer[]) (void)=
	{
		EulerAnlgesSection,
		SetEulerAnglesSection,
		HeightSection,
		OnBoardTemperatureSection,
		BatterySection,
		MotorsThrustSection,
		ControlTypeSection,
		EnviromentSensorSection,
		PIDValuesSection,
		PIDErrorValuesSection
	};
	
	void VfnMessageLimiters (void)
	{
		DataGUIBuffer[0] = '<';
		DataGUIBuffer[1] = 'Q';
		DataGUIBuffer[2] = 'X';
		if( MessageType == QXST )
		{
			DataGUIBuffer[3]='S';
			DataGUIBuffer[4]='T';
		}
		if( MessageType == QXIN )
		{
			DataGUIBuffer[3]='I';
			DataGUIBuffer[4]='N';
		}
		if( MessageType == QXDB )
		{
			DataGUIBuffer[3]='D';
			DataGUIBuffer[4]='B';
		}
		if( MessageType == QXTI )
		{
			DataGUIBuffer[3]='T';
			DataGUIBuffer[4]='I';
		}
		DataGUIBuffer[5] = ',';
		DataGUIBuffer[MESSAGE_LENGTH-1] = '>';
	}
#endif

/*****************************************************************************
 * Function:	VfnGUIBufferErase
 * Description:	Function that Erase DataGUIBuffer and, be ready to be fill it with another Message Type.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void VfnGUIBufferErase (void)
	{
		long Counter=0;
		char *DataGUIBufferPtr;
		DataGUIBufferPtr = &DataGUIBuffer[0];
		while(Counter != MESSAGE_LENGTH)
		{
			Counter++;
			*DataGUIBufferPtr = 0;
			DataGUIBufferPtr++;	
		}
	}
	
/*****************************************************************************
 * Function:	VfnParsedData
 * Description:	Function that split the Messag and, gives number of data splited and position
 * Parameters:	*BufferPtr -> Pointer to the Data buffer to be parsed
 * 				Slpiter    -> character that will split data
 * 				*DataCounter  -> Counts the number of data sliped
 * 				*DataPosition -> Buffer cointaning the beginning of the each data
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void VfnParsedData(char *BufferPtr, unsigned char ParseCounter,char Spliter, unsigned char* DataCounter, char *DataPosition)
	{
		unsigned char Counter=0;
		*DataPosition = Counter;
		DataPosition++;
		while( Counter != ParseCounter )	
		{
			if(*BufferPtr == Spliter)
			{
				*DataPosition = Counter + 1;
				DataPosition++;
				BufferPtr++;
				*DataCounter +=1;	
			}
			else
			{	
				BufferPtr++;
			}
			Counter++;
		}
	}
